import pyzed.sl as sl
import math
import cv2.cv2 as cv
# Create a ZED camera
zed = sl.Camera()
# Create configuration parameters
init_params = sl.InitParameters()
init_params.sdk_verbose = True # Enable the verbose modeinit_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE 
init_params.camera_resolution = sl.RESOLUTION.HD1080 # Use HD1080 video mode
init_params.camera_fps = 30 # Set fps at 30
# Set the depth mode to performance (fastest)
# Open the camera
err = zed.open(init_params)
if (err!=sl.ERROR_CODE.SUCCESS):
    exit(-1)

# Capture 50 images and depth, then stop
i = 0
image = sl.Mat()
depth = sl.Mat()
point_cloud = sl.Mat()
left_img = sl.Mat()
runtimeParas = sl.RuntimeParameters() 
while (i < 50000) :
    # Grab an image    
    if (zed.grab() == sl.ERROR_CODE.SUCCESS) :
        # A new image is available if grab() returns SUCCESS        
        zed.retrieve_image(image, sl.VIEW.LEFT) 
        # Get the left image        
        zed.retrieve_measure(depth, sl.MEASURE.DEPTH) 
        # Retrieve depth Mat. Depth is aligned on the left image
        zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)          
        # Retrieve colored point cloud. Point cloud is aligned on the left image     
        i = i + 1

        # Get and print distance value in mm at the center of the image
        # We measure the distance camera - object using Euclidean distance
        x = image.get_width() / 2
        y = image.get_height() / 2
        point_cloud_value = point_cloud.get_value(x, y)[1]
        distance = math.sqrt(point_cloud_value[0]*point_cloud_value[0] + point_cloud_value[1]*point_cloud_value[1] + point_cloud_value[2]*point_cloud_value[2])
        if zed.grab(runtimeParas) == sl.ERROR_CODE.SUCCESS:
            zed.retrieve_image(left_img, view=sl.VIEW.LEFT)
            time_stamp = zed.get_timestamp(sl.TIME_REFERENCE.CURRENT)
            print(time_stamp.get_milliseconds())            
            img = left_img.get_data()            
            cv.imshow("img", img)            
            cv.waitKey(5)
        print("Distance to Camera at (", x, y, "): ", distance, "mm")
        print(x,y)
zed.close()